/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_StarSensor.c
 *
 *  Created on: 2022年7月30日
 *      Author: 64435
 */

#include "LOS_StarSensor.h"
#include "LOS_SYS.h"

LOS_SSSTA_t STA_SSY;
LOS_SSSTA_t STA_SSZ;

uint8_t TMPACK_SSYShort[72];
uint8_t TMPACK_SSYLong[471];
uint8_t TMPACK_SSYCheck[16];
uint8_t TMPACK_SSYArg[112];
uint8_t TMPACK_SSYImage[419];

uint8_t TMPACK_SSZShort[72];
uint8_t TMPACK_SSZLong[471];
uint8_t TMPACK_SSZCheck[16];
uint8_t TMPACK_SSZArg[112];
uint8_t TMPACK_SSZImage[419];


uint8_t StarCMD_Reset[7]={0X3C,0XA1,0X51,0X00,0X00,0X00,0X00};        //星敏复位
uint8_t StarCMD_Check[7]={0X3C,0XA1,0X1A,0X00,0X00,0X00,0X00};        //星敏自检，会在10s内回复状态遥测
uint8_t StarCMD_Standby[7]={0X3C,0X1A,0X01,0X00,0X00,0X00,0X00};      //星敏待机
uint8_t StarCMD_Attitude[7]={0X3C,0X1A,0X10,0X00,0X00,0X00,0X00};     //进入姿态测量模式
uint8_t StarCMD_Short[7]={0X3C,0X22,0X01,0X00,0X00,0X00,0X00};        //请求数据简包
uint8_t StarCMD_Long[7]={0X3C,0X22,0X10,0X00,0X00,0X00,0X00};         //请求数据详包
uint8_t StarCMD_PAR[7]={0X3C,0X22,0XAA,0X00,0X00,0X00,0X00};          //参数下传
//由于星敏设计问题，需要在刚上电时进行一次配置以解决星敏间接性无效的问题，该指令的内容是将星敏最小像素个数由默认的5修改为10
uint8_t StarCMD_Correct[7] = {0x3C, 0X14, 0X15, 0X11, 0X0A, 0XFF, 0X00};


uint8_t LOS_StarSensor_CMD(uint8_t StarSensor_NUM, uint8_t* CMD)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    
   
    SNP_CMD.SNP_dest = StarSensor_NUM;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0xF;                //内容识别码
    SNP_CMD.SNP_len = 15;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0XFF), CMD, 7);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0XFF), CMD, 7);
}


uint8_t LOS_StarSensor_SetTime(uint8_t StarSensor_NUM, uint8_t* arg)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint8_t CMD[9] = {0X3C,0X53,0X74,0X00,0X00,0X00,0X00,0X00,0X00};
    
    SNP_CMD.SNP_dest = StarSensor_NUM;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0xF;                //内容识别码
    SNP_CMD.SNP_len = 8+9;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    memcpy(&CMD[3], arg, 6);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X01), CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X01), CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0XFF), &CMD[8], 1);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0XFF), &CMD[8], 1);
}

uint8_t LOS_StarSensor_IMG(uint8_t StarSensor_NUM, uint8_t* arg)
{
    LOS_SNP_t SNP_CMD;        //首部配置结构体
    uint8_t FP_CMD[8];        //首部内容
    uint8_t CMD[9] = {0X3C,0X22,0X55,0X00,0X00,0X00,0X00,0X00,0X00};
    
    SNP_CMD.SNP_dest = StarSensor_NUM;   
    SNP_CMD.SNP_source = 0xF0;             //OBC发送
    SNP_CMD.SNP_rec = 0xF;                //内容识别码
    SNP_CMD.SNP_len = 8+9;                 //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;               //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);    //生成首部内容
    memcpy(&CMD[3], arg, 6);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X00), FP_CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X01), CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0X01), CMD, 8);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0XFF), &CMD[8], 1);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0xF0, StarSensor_NUM, 0XFF), &CMD[8], 1);
}

void LOS_CAN_SSYCallBack(LOS_CANMSG_t* can_re)
{
    /* 首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃 */
    uint8_t id_filter[4];
    uint8_t sum = 0;
    uint8_t sum_check = 0;
    static uint8_t canerrflag = 0;     //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t canmsgrec = 0xFF;   //消息类型（对应首部的rec）
    int32_t buf = 0;                   //四元数/角速度缓冲区
    static uint8_t break_buf[4];     //四元数/角速度断帧缓冲区（一个四元数被分成两帧传输）
    uint32_t sta_buf;
    uint8_t sta_buf_g[4];

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理

    if (id_filter[3]==0x00)                //若为首帧
    {
        STA_SSY.HL_TXF = 0;
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            canmsgrec = can_re->msg[6];  
        }
        else 
        {
           canerrflag = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (canmsgrec == SSMSG_SP)       //数据简包
            {
                memcpy(&(STA_SSY.MODEC[2]), &(can_re->msg[0]), 2); //工作状态C
                memcpy(&TMPACK_SSYShort[66], &can_re->msg[0], 6);
            }
            else if (canmsgrec == SSMSG_LP)  //数据详包
            {
                memcpy(&TMPACK_SSYLong[466], &can_re->msg[0], 6);
            }
            else if (canmsgrec == SSMSG_SC)  //自检遥测
            {
                if (can_re->msg[0] != 0x3C || can_re->msg[1] != 0xA1 || can_re->msg[2] != 0x1A)
                {
                    canerrflag = 1;      //最后一帧了，不置位其实也没啥事，但是我乐意，略略略~
                }
                else
                {
                    STA_SSY.temp = can_re->msg[3];
                    memcpy(STA_SSY.scsta, &can_re->msg[3], 3);
                }
                memcpy(&TMPACK_SSYCheck[0], &LOS_Time_SecFromBoot, 4);
                memcpy(&TMPACK_SSYCheck[4], &LOS_Time_MilSec, 2);
                memcpy(&TMPACK_SSYCheck[6], &LOS_Time_Sec, 4);
                memcpy(&TMPACK_SSYCheck[10], &can_re->msg[1], 6);
            }
            else if (canmsgrec == SSMSG_AG)  //参数下传
            {
                /* 地面调试用，OBC不处理 */
                memcpy(&TMPACK_SSYArg[105], &can_re->msg[0], 7);
            }
            else if (canmsgrec == SSMSG_SM)  //星图遥测
            {
                TMPACK_SSYImage[418] = can_re->msg[0];
            }
            STA_SSY.TMCNT++;
            
        }
        canerrflag = 0;   //最后一帧时清除标志位
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (canmsgrec == SSMSG_SP)     //数据简包
            {
                if (id_filter[3]==0x01)
                {
                    if(can_re->msg[0] != 0x3C || can_re->msg[1] != 0xF1 || can_re->msg[2] != 0x90)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        /* 什么也不做 */
                    }
                    memcpy(&TMPACK_SSYShort[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSYShort[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSYShort[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSYShort[10], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x02)
                {
                    if(can_re->msg[0] != 0x22 || can_re->msg[1] != 0x01)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        memcpy(&STA_SSY.timestamp_sec, &(can_re->msg[2]), 4);
                        memcpy(&STA_SSY.timestamp_milsec, &(can_re->msg[6]), 2);
                        memcpy(&TMPACK_SSYShort[18], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x03)
                {
                    memcpy(&buf, &(can_re->msg[2]), 4);     //四元数0
                    memcpy(&STA_SSY.q_o[0], &(can_re->msg[2]), 4);    
                    STA_SSY.q[0] = (double)buf/1073741824;          
                    memcpy(break_buf, &(can_re->msg[6]), 2);//四元数1前两字节
                    memcpy(&TMPACK_SSYShort[26], &can_re->msg[0], 8);  
                }
                else if (id_filter[3]==0x04)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数1后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数1
                    memcpy(&STA_SSY.q_o[1], break_buf, 4);
                    STA_SSY.q[1] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //四元数2
                    memcpy(&STA_SSY.q_o[2], &(can_re->msg[2]), 4);
                    STA_SSY.q[2] = (double)buf/1073741824;                   //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //四元数3前两字节
                    memcpy(&TMPACK_SSYShort[34], &can_re->msg[0], 8);  
                }
                else if (id_filter[3]==0x05)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数3后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数3
                    memcpy(&STA_SSY.q_o[3], break_buf, 4);
                    STA_SSY.q[3] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //X轴角速度
                    memcpy(&STA_SSY.angvel_o[0], &(can_re->msg[2]), 4);
                    STA_SSY.angvel[0] = (double)buf/1073741824;              //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //Y轴角速度前两字节
                    memcpy(&TMPACK_SSYShort[42], &can_re->msg[0], 8);                    
                }
                else if (id_filter[3]==0x06)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//Y轴角速度后两字节
                    memcpy(&buf, break_buf, 4);                 //Y轴角速度
                    memcpy(&STA_SSY.angvel_o[1], break_buf, 4);
                    STA_SSY.angvel[1] = (double)buf/1073741824;              //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //Z轴角速度
                    memcpy(&STA_SSY.angvel_o[2], &(can_re->msg[2]), 4);
                    STA_SSY.angvel[2] = (double)buf/1073741824;              //float转double
                    memcpy(&(STA_SSY.MODEA[0]), &(can_re->msg[6]), 2);//工作状态A
                    memcpy(&TMPACK_SSYShort[50], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x07)
                {
                    memcpy(&(STA_SSY.MODEA[2]), &(can_re->msg[0]), 2);//工作状态A
                    memcpy(&(STA_SSY.MODEB[0]), &(can_re->msg[2]), 4);//工作状态B
                    memcpy(&(STA_SSY.MODEC[0]), &(can_re->msg[6]), 2);//工作状态C
                    memcpy(&sta_buf, &(STA_SSY.MODEA[0]), 4);
                    sta_buf = sta_buf << 12;
                    sta_buf = sta_buf >> 26;
                    memcpy(sta_buf_g, &sta_buf, 4);
                    STA_SSY.StarNum = sta_buf_g[3];                   //识别星个数
                    memcpy(&sta_buf, &(STA_SSY.MODEA[0]), 4);
                    memcpy(&TMPACK_SSYShort[58], &can_re->msg[0], 8);
                    if (0x10000000 & sta_buf)
                    {
                        STA_SSY.Flag_D = 1;
                    }
                    else
                    {
                        STA_SSY.Flag_D = 0;
                    }
                }
            }
            else if (canmsgrec == SSMSG_LP)   //数据详包
            {
                if (id_filter[3]==0x01)
                {
                    if(can_re->msg[0] != 0x3C || can_re->msg[1] != 0xF1 || can_re->msg[2] != 0x90)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        /* 什么也不做 */
                    }
                    memcpy(&TMPACK_SSYLong[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSYLong[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSYLong[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSYLong[10], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x02)
                {
                    if(can_re->msg[0] != 0x22 || can_re->msg[1] != 0x10)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        memcpy(&STA_SSY.timestamp_sec, &(can_re->msg[2]), 4);    //秒时间戳
                        memcpy(&STA_SSY.timestamp_milsec, &(can_re->msg[6]), 2); //毫秒时间戳
                        STA_SSY.timestamp = STA_SSY.timestamp_sec + 0.001 * STA_SSY.timestamp_milsec;  //小数时间戳
                        memcpy(&TMPACK_SSYLong[18], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x03)
                {
                    memcpy(&buf, &(can_re->msg[2]), 4);     //四元数0
                    memcpy(&STA_SSY.q_o[0], &(can_re->msg[2]), 4);    
                    STA_SSY.q[0] = (double)buf/1073741824;          
                    memcpy(break_buf, &(can_re->msg[6]), 2);//四元数1前两字节
                    memcpy(&TMPACK_SSYShort[26], &can_re->msg[0], 8);  
                }
                else if (id_filter[3]==0x04)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数1后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数1
                    memcpy(&STA_SSY.q_o[1], break_buf, 4);
                    STA_SSY.q[1] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //四元数2
                    memcpy(&STA_SSY.q_o[2], &(can_re->msg[2]), 4);
                    STA_SSY.q[2] = (double)buf/1073741824;                   //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //四元数3前两字节
                    memcpy(&TMPACK_SSYShort[34], &can_re->msg[0], 8);  
                }
                else if (id_filter[3]==0x05)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数3后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数3
                    memcpy(&STA_SSY.q_o[3], break_buf, 4);
                    STA_SSY.q[3] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //X轴角速度
                    memcpy(&STA_SSY.angvel_o[0], &(can_re->msg[2]), 4);
                    STA_SSY.angvel[0] = (double)buf/1073741824;              //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //Y轴角速度前两字节
                    memcpy(&TMPACK_SSYShort[42], &can_re->msg[0], 8);                    
                }
                else if (id_filter[3]==0x06)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//Y轴角速度后两字节
                    memcpy(&buf, break_buf, 4);                 //Y轴角速度
                    memcpy(&STA_SSY.angvel_o[1], break_buf, 4);
                    STA_SSY.angvel[1] = (double)buf/1073741824;              //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //Z轴角速度
                    memcpy(&STA_SSY.angvel_o[2], &(can_re->msg[2]), 4);
                    STA_SSY.angvel[2] = (double)buf/1073741824;              //float转double
                    memcpy(&(STA_SSY.MODEA[0]), &(can_re->msg[6]), 2);//工作状态A
                    memcpy(&TMPACK_SSYShort[50], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x07)
                {
                    memcpy(&(STA_SSY.MODEA[2]), &(can_re->msg[0]), 2);//工作状态A
                    memcpy(&(STA_SSY.MODEB[0]), &(can_re->msg[2]), 4);//工作状态B
                    memcpy(&(STA_SSY.MODEC[0]), &(can_re->msg[6]), 2);//工作状态C
                    memcpy(&sta_buf, &(STA_SSY.MODEA[0]), 4);
                    sta_buf = sta_buf << 12;
                    sta_buf = sta_buf >> 26;
                    memcpy(sta_buf_g, &sta_buf, 4);
                    STA_SSY.StarNum = sta_buf_g[3];                   //观测星个数
                    memcpy(&sta_buf, &(STA_SSY.MODEA[0]), 4);
                    memcpy(&TMPACK_SSYLong[58], &can_re->msg[0], 8);
                    if (0x10000000 & sta_buf)
                    {
                        STA_SSY.Flag_D = 1;
                    }
                    else
                    {
                        STA_SSY.Flag_D = 0;
                    }
                }
                else
                {
                    memcpy(&TMPACK_SSYLong[10 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }
            }
            else if (canmsgrec == SSMSG_SC)   //自检遥测
            {
                /* 自检遥测无中间帧 */
            }
            else if (canmsgrec == SSMSG_AG)   //参数下传
            {
                /* 地面调试用，OBC不处理 */
                if (id_filter[3]==0x01)
                {
                    memcpy(&TMPACK_SSYArg[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSYArg[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSYArg[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSYArg[10], &can_re->msg[1], 7);
                }
                else
                {
                    memcpy(&TMPACK_SSYArg[9 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }

            }
            else if (canmsgrec == SSMSG_SM)   //星图遥测
            {
                if (id_filter[3]==0x01)
                {
                    memcpy(&TMPACK_SSYImage[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSYImage[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSYImage[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSYImage[10], &can_re->msg[0], 8);
                }
                else
                {
                    memcpy(&TMPACK_SSYImage[10 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }
            }

        }
    }
}


void LOS_CAN_SSZCallBack(LOS_CANMSG_t* can_re)
{
    /* 首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃 */
    uint8_t id_filter[4];
    uint8_t sum = 0;
    uint8_t sum_check = 0;
    static uint8_t canerrflag = 0;     //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t canmsgrec = 0xFF;   //消息类型（对应首部的rec）
    int32_t buf = 0;                   //四元数/角速度缓冲区
    static uint8_t break_buf[4];     //四元数/角速度断帧缓冲区（一个四元数被分成两帧传输）
    uint32_t sta_buf;
    uint8_t sta_buf_g[4];

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理

    if (id_filter[3]==0x00)                //若为首帧
    {
        STA_SSZ.HL_TXF = 0;
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            canmsgrec = can_re->msg[6];  
        }
        else 
        {
           canerrflag = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (canmsgrec == SSMSG_SP)       //数据简包
            {
                memcpy(&(STA_SSZ.MODEC[2]), &(can_re->msg[0]), 2); //工作状态C
                memcpy(&TMPACK_SSZShort[66], &can_re->msg[0], 6);
            }
            else if (canmsgrec == SSMSG_LP)  //数据详包
            {
                memcpy(&TMPACK_SSZLong[466], &can_re->msg[0], 6);
            }
            else if (canmsgrec == SSMSG_SC)  //自检遥测
            {
                if (can_re->msg[0] != 0x3C || can_re->msg[1] != 0xA1 || can_re->msg[2] != 0x1A)
                {
                    canerrflag = 1;      //最后一帧了，不置位其实也没啥事，但是我乐意，略略略~
                }
                else
                {
                    STA_SSZ.temp = can_re->msg[3];
                    memcpy(STA_SSZ.scsta, &can_re->msg[3], 3);
                }
                memcpy(&TMPACK_SSZCheck[0], &LOS_Time_SecFromBoot, 4);
                memcpy(&TMPACK_SSZCheck[4], &LOS_Time_MilSec, 2);
                memcpy(&TMPACK_SSZCheck[6], &LOS_Time_Sec, 4);
                memcpy(&TMPACK_SSZCheck[10], &can_re->msg[1], 6);
            }
            else if (canmsgrec == SSMSG_AG)  //参数下传
            {
                /* 地面调试用，OBC不处理 */
                memcpy(&TMPACK_SSZArg[105], &can_re->msg[0], 7);
            }
            else if (canmsgrec == SSMSG_SM)  //星图遥测
            {
                TMPACK_SSZImage[418] = can_re->msg[0];
            }
            STA_SSZ.TMCNT++;
        }
        canerrflag = 0;   //最后一帧时清除标志位
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (canmsgrec == SSMSG_SP)     //数据简包
            {
                if (id_filter[3]==0x01)
                {
                    if(can_re->msg[0] != 0x3C || can_re->msg[1] != 0xF1 || can_re->msg[2] != 0x90)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        /* 什么也不做 */
                    }
                    memcpy(&TMPACK_SSZShort[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSZShort[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSZShort[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSZShort[10], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x02)
                {
                    if(can_re->msg[0] != 0x22 || can_re->msg[1] != 0x01)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        memcpy(&STA_SSZ.timestamp_sec, &(can_re->msg[2]), 4);
                        memcpy(&STA_SSZ.timestamp_milsec, &(can_re->msg[6]), 2);
                        memcpy(&TMPACK_SSZShort[18], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x03)
                {
                    memcpy(&buf, &(can_re->msg[2]), 4);     //四元数0
                    memcpy(&STA_SSZ.q_o[0], &(can_re->msg[2]), 4);
                    STA_SSZ.q[0] = (double)buf/1073741824;               //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);//四元数1前两字节
                    memcpy(&TMPACK_SSZShort[26], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x04)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数1后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数1
                    memcpy(&STA_SSZ.q_o[1], break_buf, 4);
                    STA_SSZ.q[1] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //四元数2
                    memcpy(&STA_SSZ.q_o[2], &(can_re->msg[2]), 4);
                    STA_SSZ.q[2] = (double)buf/1073741824;                   //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //四元数3前两字节
                    memcpy(&TMPACK_SSZShort[34], &can_re->msg[0], 8);  
                }
                else if (id_filter[3]==0x05)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数3后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数3
                    memcpy(&STA_SSZ.q_o[3], break_buf, 4);
                    STA_SSZ.q[3] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //X轴角速度
                    memcpy(&STA_SSZ.angvel_o[0], &(can_re->msg[2]), 4);
                    STA_SSZ.angvel[0] = (double)buf/1073741824;              //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //Y轴角速度前两字节
                    memcpy(&TMPACK_SSZShort[42], &can_re->msg[0], 8);                    
                }
                else if (id_filter[3]==0x06)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//Y轴角速度后两字节
                    memcpy(&buf, break_buf, 4);                 //Y轴角速度
                    memcpy(&STA_SSZ.angvel_o[1], break_buf, 4);
                    STA_SSZ.angvel[1] = (double)buf/1073741824;              //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //Z轴角速度
                    memcpy(&STA_SSZ.angvel_o[2], &(can_re->msg[2]), 4);
                    STA_SSZ.angvel[2] = (double)buf/1073741824;              //float转double
                    memcpy(&(STA_SSZ.MODEA[0]), &(can_re->msg[6]), 2);//工作状态A
                    memcpy(&TMPACK_SSZShort[50], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x07)
                {
                    memcpy(&(STA_SSZ.MODEA[2]), &(can_re->msg[0]), 2);//工作状态A
                    memcpy(&(STA_SSZ.MODEB[0]), &(can_re->msg[2]), 4);//工作状态B
                    memcpy(&(STA_SSZ.MODEC[0]), &(can_re->msg[6]), 2);//工作状态C
                    memcpy(&sta_buf, &(STA_SSZ.MODEA[0]), 4);
                    sta_buf = sta_buf << 12;
                    sta_buf = sta_buf >> 26;
                    memcpy(sta_buf_g, &sta_buf, 4);
                    STA_SSZ.StarNum = sta_buf_g[3];                   //观测星个数
                    memcpy(&sta_buf, &(STA_SSZ.MODEA[0]), 4);
                    memcpy(&TMPACK_SSZShort[58], &can_re->msg[0], 8);
                    if (0x10000000 & sta_buf)
                    {
                        STA_SSZ.Flag_D = 1;
                    }
                    else
                    {
                        STA_SSZ.Flag_D = 0;
                    }
                }
            }
            else if (canmsgrec == SSMSG_LP)   //数据详包
            {
                if (id_filter[3]==0x01)
                {
                    if(can_re->msg[0] != 0x3C || can_re->msg[1] != 0xF1 || can_re->msg[2] != 0x90)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        /* 什么也不做 */
                    }
                    memcpy(&TMPACK_SSZLong[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSZLong[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSZLong[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSZLong[10], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x02)
                {
                    if(can_re->msg[0] != 0x22 || can_re->msg[1] != 0x10)
                    {
                        canerrflag = 1;
                    }
                    else
                    {
                        memcpy(&STA_SSZ.timestamp_sec, &(can_re->msg[2]), 4);
                        memcpy(&STA_SSZ.timestamp_milsec, &(can_re->msg[6]), 2);
                        STA_SSZ.timestamp = STA_SSZ.timestamp_sec + 0.001 * STA_SSZ.timestamp_milsec;  //小数时间戳
                        memcpy(&TMPACK_SSZLong[18], &can_re->msg[0], 8);
                    }
                }
                else if (id_filter[3]==0x03)
                {
                    memcpy(&buf, &(can_re->msg[2]), 4);     //四元数0
                    memcpy(&STA_SSZ.q_o[0], &(can_re->msg[2]), 4);
                    STA_SSZ.q[0] = (double)buf/1073741824;               //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);//四元数1前两字节
                    memcpy(&TMPACK_SSZShort[26], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x04)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数1后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数1
                    memcpy(&STA_SSZ.q_o[1], break_buf, 4);
                    STA_SSZ.q[1] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //四元数2
                    memcpy(&STA_SSZ.q_o[2], &(can_re->msg[2]), 4);
                    STA_SSZ.q[2] = (double)buf/1073741824;                   //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //四元数3前两字节
                    memcpy(&TMPACK_SSZShort[34], &can_re->msg[0], 8);  
                }
                else if (id_filter[3]==0x05)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//四元数3后两字节
                    memcpy(&buf, break_buf, 4);                 //四元数3
                    memcpy(&STA_SSZ.q_o[3], break_buf, 4);
                    STA_SSZ.q[3] = (double)buf/1073741824;                   //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //X轴角速度
                    memcpy(&STA_SSZ.angvel_o[0], &(can_re->msg[2]), 4);
                    STA_SSZ.angvel[0] = (double)buf/1073741824;              //float转double
                    memcpy(break_buf, &(can_re->msg[6]), 2);    //Y轴角速度前两字节
                    memcpy(&TMPACK_SSZShort[42], &can_re->msg[0], 8);                    
                }
                else if (id_filter[3]==0x06)
                {
                    memcpy(&break_buf[2], &(can_re->msg[0]), 2);//Y轴角速度后两字节
                    memcpy(&buf, break_buf, 4);                 //Y轴角速度
                    memcpy(&STA_SSZ.angvel_o[1], break_buf, 4);
                    STA_SSZ.angvel[1] = (double)buf/1073741824;              //float转double
                    memcpy(&buf, &(can_re->msg[2]), 4);         //Z轴角速度
                    memcpy(&STA_SSZ.angvel_o[2], &(can_re->msg[2]), 4);
                    STA_SSZ.angvel[2] = (double)buf/1073741824;              //float转double
                    memcpy(&(STA_SSZ.MODEA[0]), &(can_re->msg[6]), 2);//工作状态A
                    memcpy(&TMPACK_SSZShort[50], &can_re->msg[0], 8);
                }
                else if (id_filter[3]==0x07)
                {
                    memcpy(&(STA_SSZ.MODEA[2]), &(can_re->msg[0]), 2);//工作状态A
                    memcpy(&(STA_SSZ.MODEB[0]), &(can_re->msg[2]), 4);//工作状态B
                    memcpy(&(STA_SSZ.MODEC[0]), &(can_re->msg[6]), 2);//工作状态C
                    memcpy(&sta_buf, &(STA_SSZ.MODEA[0]), 4);
                    sta_buf = sta_buf << 12;
                    sta_buf = sta_buf >> 26;
                    memcpy(sta_buf_g, &sta_buf, 4);
                    STA_SSZ.StarNum = sta_buf_g[3];                   //观测星个数
                    memcpy(&sta_buf, &(STA_SSZ.MODEA[0]), 4);
                    memcpy(&TMPACK_SSZLong[58], &can_re->msg[0], 8);
                    if (0x10000000 & sta_buf)
                    {
                        STA_SSZ.Flag_D = 1;
                    }
                    else
                    {
                        STA_SSZ.Flag_D = 0;
                    }
                }
                else
                {
                    memcpy(&TMPACK_SSZLong[10 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }
            }
            else if (canmsgrec == SSMSG_SC)   //自检遥测
            {
                /* 自检遥测无中间帧 */
            }
            else if (canmsgrec == SSMSG_AG)   //参数下传
            {
                /* 地面调试用，OBC不处理 */
                if (id_filter[3]==0x01)
                {
                    memcpy(&TMPACK_SSZArg[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSZArg[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSZArg[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSZArg[10], &can_re->msg[1], 7);
                }
                else
                {
                    memcpy(&TMPACK_SSZArg[9 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }
            }
            else if (canmsgrec == SSMSG_SM)   //星图遥测
            {
                if (id_filter[3]==0x01)
                {
                    memcpy(&TMPACK_SSZImage[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_SSZImage[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_SSZImage[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_SSZImage[10], &can_re->msg[0], 8);
                }
                else
                {
                    memcpy(&TMPACK_SSZImage[10 + 8*(id_filter[3]-1)], &can_re->msg[0], 8);
                }
            }

        }
    }
}

void LOS_SSY_PWROn()
{
    LOS_EPS_TCSwitch(13, SWITCH_ON);
    STA_SSY.CMD = 1;
}

void LOS_SSY_PWROff()
{
    LOS_EPS_TCSwitch(13, SWITCH_OFF);
    STA_SSY.MSG = 1;
    STA_SSY.CMD = 0;
}

void LOS_SSZ_PWROn()
{
    LOS_EPS_TCSwitch(14, SWITCH_ON);
    STA_SSZ.CMD = 1;
}

void LOS_SSZ_PWROff()
{
    LOS_EPS_TCSwitch(14, SWITCH_OFF);
    STA_SSZ.MSG = 1;
    STA_SSZ.CMD = 0;
}
